// This is the main DLL file.

#include "stdafx.h"
#include "Rotate3D.h"
#include <cmath>
namespace Rotate3dN
{
    using namespace std;
	void ExpToMatrix(Matrix3dC& R, RealT x, RealT y, RealT z)
	{
		RealT ang, c, v, s;
		ang = sqrt(x*x + y*y + z*z);

		if (ang > axis_threshold)
		{ 
			x = x/ang; y = y/ang; z = z/ang;
			c = cos(ang);
			s = sin(ang);
			v = 1 - c;
		}
		else 
		{ 
			c = 1.0 - 0.50000000*ang*ang;
			v = 0.5 - 0.04166667*ang*ang;
			s = 1.0 - 0.16666667*ang*ang;
		}

		R[0][0] = x*x*v + c;   R[0][1] = x*y*v - z*s; R[0][2] = x*z*v + y*s;
		R[1][0] = x*y*v + z*s; R[1][1] = y*y*v + c;   R[1][2] = y*z*v - x*s;
		R[2][0] = x*z*v - y*s; R[2][1] = y*z*v + x*s; R[2][2] = z*z*v + c;
	}

	void MatrixToExp(Vector3dC& a, const Matrix3dC& R)
	{
		RealT cos_ang, ang, s;

		cos_ang = 0.5 * (R[0][0]+R[1][1]+R[2][2]-1.0);
		if (cos_ang > 1.0)
			cos_ang = 1.0;
		else if (cos_ang < -1.0)
			cos_ang = -1.0;

		ang = acos(cos_ang);

		if (ang < -axis_threshold || ang > axis_threshold)
		{
			s = 0.5 * ang / sin(ang);
		}
		else
		{ 
			// ARRGH! causes problems - should use small-angle implementation
			// IKL_VEC_SET_3(a, 0.0);
			s = 0.5;
		}

		a[0] = (R[2][1]-R[1][2])*s;
		a[1] = (R[0][2]-R[2][0])*s;
		a[2] = (R[1][0]-R[0][1])*s;
	}

	void ExpToMatrix_J(Matrix3dC& dR, const Vector3dC& axis, UIntT dParam)
	{
		RealT x, y, z, ang, a, b, c, d, alpha, vx, vy, vz;

		x = axis[0];
		y = axis[1];
		z = axis[2];
		ang = sqrt(x*x + y*y + z*z);

		// Calculate values used for derivatives
		if (ang > axis_threshold)
		{
			a = (sin(ang)/ang);
			b = (cos(ang)/(ang*ang) - sin(ang)/(ang*ang*ang));
			c = (sin(ang)/(ang*ang*ang) - 2*(1-cos(ang))/(ang*ang*ang*ang));
			d = ((1-cos(ang))/(ang*ang));
		}
		else  // Small angle implementation
		{
			a = 1.0;
			b = -0.33333333;
			c = -0.83333333;
			d = 0.5;

			// dR[0] = 0.0;  dR[1] = 

			// a = (1-(ang*ang)/6.0);
			// b = (-1.0/3.0 + (ang*ang)/30.0);
			// c = (-1.0/12.0 + (ang*ang)/180.0);
			// d = (0.5 - (ang*ang)/24.0);
		}

		// Calculate the rotation derivative
		alpha = axis[dParam];
		vx = (dParam == 0) ? 1 : 0;
		vy = (dParam == 1) ? 1 : 0;
		vz = (dParam == 2) ? 1 : 0;

		dR[0][0] = -(z*z+y*y)*c*(alpha) - 2.0*(z*vz+y*vy)*d;
		dR[0][1] = -vz*a - z*b*(alpha) + x*y*c*(alpha) + (x*vy+y*vx)*d;
		dR[0][2] = vy*a + y*b*(alpha) + x*z*c*(alpha) + (x*vz+z*vx)*d;

		dR[1][0] = vz*a + z*b*(alpha) + x*y*c*(alpha) + (x*vy+y*vx)*d;
		dR[1][1] = -(x*x+z*z)*c*(alpha) -2.0*(x*vx+z*vz)*d;
		dR[1][2] = -vx*a - x*b*(alpha) + z*y*c*(alpha) + (z*vy+y*vz)*d;

		dR[2][0] = -vy*a - y*b*(alpha) + x*z*c*(alpha) + (x*vz+z*vx)*d;
		dR[2][1] = vx*a + x*b*(alpha) + z*y*c*(alpha) + (z*vy+y*vz)*d;
		dR[2][2] = -(x*x+y*y)*c*(alpha) - 2.0*(x*vx+y*vy)*d;
	}

	void QuartToMatrix(Matrix3dC& R, const Vector4dC& q)
	{
		RealT s = q[0];
		RealT l = q[1];
		RealT m = q[2];
		RealT n = q[3];

		R[0][0] = s*s + l*l - m*m - n*n; R[0][1] = 2.0*(l*m - s*n);       R[0][2] = 2.0*(l*n + s*m);
		R[1][0] = 2*(l*m + s*n);         R[1][1] = s*s - l*l + m*m - n*n; R[1][2] = 2.0*(m*n - s*l);
		R[2][0] = 2*(l*n - s*m);         R[2][1] = 2.0*(m*n + s*l);       R[2][2] = s*s - l*l - m*m + n*n;
	}

		void EulerXYZToMatrix(Matrix3dC& R, RealT a0, RealT a1, RealT a2)
		{

			RealT sa0 = sin(a0);
			RealT sa1 = sin(a1);
			RealT sa2 = sin(a2);
			RealT ca0 = cos(a0);
			RealT ca1 = cos(a1);
			RealT ca2 = cos(a2);

			R[0][0] = ca2*ca1;
			R[0][1] = ca2*sa1*sa0 - sa2*ca0;
			R[0][2] = ca2*sa1*ca0 + sa2*sa0;

			R[1][0] = sa2*ca1;
			R[1][1] = sa2*sa1*sa0 + ca2*ca0;
			R[1][2] = sa2*sa1*ca0 - ca2*sa0;

			R[2][0] = -sa1;
			R[2][1] = ca1*sa0;
			R[2][2] = ca1*ca0;

		}

		void MatrixToEulerXYZ(Vector3dC& a, const Matrix3dC& R, RealT a2_default)
		{
			a[1] = atan2(-R[2][0], sqrt(R[2][1]*R[2][1] + R[2][2]*R[2][2]));
			RealT cos_a1 = cos(a[1]);

			if (cos_a1 > -1E-12 && cos_a1 < 1E-12)
			{
				a[2] = a2_default;
				a[0] = atan2(-R[1][2], R[1][1]);
			}
			else
			{
				RealT r_cos_a1 = 1.0 / cos_a1;
				a[0] = atan2(R[2][1] * r_cos_a1, R[2][2] * r_cos_a1);
				a[2] = atan2(R[1][0] * r_cos_a1, R[0][0] * r_cos_a1);
			}
		}

		void MatrixToEulerZXY(Vector3dC& a, const Matrix3dC& R, RealT a1_default)
		{
			a[0] = atan2(-R[1][2], sqrt(R[1][0]*R[1][0] + R[1][1]*R[1][1]));
			RealT cos_a0 = cos(a[0]);

			if (cos_a0 > -1E-12 && cos_a0 < 1E-12)
			{
				a[1] = a1_default;
				a[2] = atan2(-R[0][1], -R[0][0]);
			}
			else
			{
				RealT r_cos_a0 = 1.0 / cos_a0;
				a[1] = atan2(R[0][2] * r_cos_a0, R[2][2] * r_cos_a0);
				a[2] = atan2(R[1][0] * r_cos_a0, R[1][1] * r_cos_a0);
			}
		}

		void MatrixToEulerYXZ(Vector3dC& a, const Matrix3dC& R, RealT a2_default)
		{
			a[0] = atan2(R[2][1], sqrt(R[2][0]*R[2][0] + R[2][2]*R[2][2]));
			RealT cos_a0 = cos(a[0]);

			if (cos_a0 > -1E-12 && cos_a0 < 1E-12)
			{
				a[2] = a2_default;
				a[1] = atan2(R[0][2], R[0][0]);
			}
			else
			{
				RealT r_cos_a0 = 1.0 / cos_a0;
				a[2] = atan2(-R[0][1] * r_cos_a0, R[1][1] * r_cos_a0);
				a[1] = atan2(-R[2][0] * r_cos_a0, R[2][2] * r_cos_a0);
			}
		}


		void EulerYXZToMatrix(Matrix3dC& R, RealT a0, RealT a1, RealT a2)
		{
			RealT sa0 = sin(a0);
			RealT sa1 = sin(a1);
			RealT sa2 = sin(a2);
			RealT ca0 = cos(a0);
			RealT ca1 = cos(a1);
			RealT ca2 = cos(a2);
			R[0][0] = -sa2*sa0*sa1 + ca2*ca1;
			R[0][1] = -sa2*ca0;
			R[0][2] =  sa2*sa0*ca1 + ca2*sa1;
			R[1][0] =  ca2*sa0*sa1 + sa2*ca1;
			R[1][1] =  ca2*ca0;
			R[1][2] = -ca2*sa0*ca1 + sa2*sa1;
			R[2][0] =     -ca0*sa1;
			R[2][1] =      sa0;
			R[2][2] =      ca0*ca1;
		}

		void EulerXYZToMatrix_da0(Matrix3dC& R, RealT a0, RealT a1, RealT a2)
		{
			R[0][0] =  0.0;
			R[0][1] =  cos(a2)*sin(a1)*cos(a0) + sin(a2)*sin(a0);
			R[0][2] = -cos(a2)*sin(a1)*sin(a0) + sin(a2)*cos(a0);

			R[1][0] =  0.0;
			R[1][1] =  sin(a2)*sin(a1)*cos(a0) - cos(a2)*sin(a0);
			R[1][2] = -sin(a2)*sin(a1)*sin(a0) - cos(a2)*cos(a0);

			R[2][0] = 0.0;
			R[2][1] = cos(a1)*cos(a0);
			R[2][2] = -cos(a1)*sin(a0);
		}

		void EulerXYZToMatrix_da1(Matrix3dC& R, RealT a0, RealT a1, RealT a2)
		{
			R[0][0] = -cos(a2)*sin(a1);
			R[0][1] = cos(a2)*cos(a1)*sin(a0);
			R[0][2] = cos(a2)*cos(a1)*cos(a0);

			R[1][0] = -sin(a2)*sin(a1);
			R[1][1] = sin(a2)*cos(a1)*sin(a0);
			R[1][2] = sin(a2)*cos(a1)*cos(a0);

			R[2][0] = -cos(a1);
			R[2][1] = -sin(a1)*sin(a0);
			R[2][2] = -sin(a1)*cos(a0);
		}


		void EulerXYZToMatrix_da2(Matrix3dC& R, RealT a0, RealT a1, RealT a2)
		{
			R[0][0] = -sin(a2)*cos(a1);
			R[0][1] = -sin(a2)*sin(a1)*sin(a0) - cos(a2)*cos(a0);
			R[0][2] = -sin(a2)*sin(a1)*cos(a0) + cos(a2)*sin(a0);

			R[1][0] = cos(a2)*cos(a1);
			R[1][1] = cos(a2)*sin(a1)*sin(a0) - sin(a2)*cos(a0);
			R[1][2] = cos(a2)*sin(a1)*cos(a0) + sin(a2)*sin(a0);

			R[2][0] = 0.0;
			R[2][1] = 0.0;
			R[2][2] = 0.0;
		}
	}
